% 并行程序
function [temp]=mult_test(r0,v0)
m_dry = 1505;       % Vehicle mass without fuel [kg]
Isp = 225;          % Specific impulse [s]
%g0 = 9.80665;       % Standard earth gravity [m/s^2]
g = [0 ; -3.7114];  % Mars gravity vector [m/s^2]
max_throttle = 0.9; % Max open throttle [.%]
min_throttle = 0.3; % Min open throttle [.%]
T_max = 6 * 3100;   % Max total thrust force at 1.0 throttle [N]
theta = 27;           % The cant angle of thrusters [deg]

% Initial conditions
m_wet = 1905;           % Vehicle mass with fuel [kg]
% r0 = [  2 ; 1.5] * 1e3; % Initial position [x;z] [m]
% v0 = [100 ; -75];       % Initial velocity [x;z] [m/s]

% Target conditions
rf = [ 0 ; 0 ];
vf = [ 0 ; 0 ];

p.m_dry = m_dry;
p.Isp = Isp;
p.g = g;
p.max_throttle = max_throttle;
p.min_throttle = min_throttle;
p.T_max = T_max;
p.phi = 15;
N = 50;

%[tv,m_used,r,v,u,m,s,z,s_bound_min,s_bound_max,z_bound]=GFOLD(N,r0,v0,rf,vf,m_wet,theta,p);
[tv,m_used,r,~,u,~,s,z,s_bound_min,s_bound_max,z_bound]=GFOLD(N,r0,v0,rf,vf,m_wet,theta,p);
% 判断不等式约束是否非紧约束
% alpha = 1 / (Isp * g0 * cosd(phi));
% r1 = min_throttle * T_max * cosd(phi);
% r2 = max_throttle * T_max * cosd(phi);
%% 紧约束计算
T_constraint = zeros(N,1);
%S_constraint = zeros(N,2);
S_constraint_min = zeros(N,1);
S_constraint_max = zeros(N,1);
Z_constraint_min = zeros(N,1);
Z_constraint_max = zeros(N,1);
Point_constraint = zeros(N,1);
UnderFly_constraint = zeros(N,1);
u_min_bound = s .* cosd(theta);
for i = 1:1:N
    % 推力约束
    T_constraint(i,1) = my_equal(norm(u(:,i)),s(i));
    % 推力松弛变量约束
    S_constraint_min(i,1) = my_equal(s(i),s_bound_min(i));
    S_constraint_max(i,2) = my_equal(s(i),s_bound_max(i));
    % 物理质量上下界
    Z_constraint_min(i,1) = my_equal(z(i),z_bound(1));
    Z_constraint_max(i,2) = my_equal(z(i),z_bound(2));
    %推力指向约束
    Point_constraint(i,1) = my_equal(u(2,i),u_min_bound(1,i));
    
end



%地下飞行约束
for i = 1:1:N-1
    UnderFly_constraint(i,1) = my_equal(r(2,i),0);
end
t_final = tv(N);
if(m_used == 1905)
    t_final  = -1;
end
temp = {t_final,T_constraint,S_constraint_min,S_constraint_max, ...
        Z_constraint_min,Z_constraint_max,Point_constraint,UnderFly_constraint};
end

